#include "math/cubic_interpolation.h"
namespace vehicle {
namespace common {
namespace math {


CubicSpline::CubicSpline(std::vector<double> x, std::vector<double> y) {
  Build(x, y);
}

void CubicSpline::Build(const std::vector<double>& x,
                        const std::vector<double>& y) {
  x_ = x;
  y_ = y;
  num_point_ = y_.size();

  // calculate vector neighbor element difference
  std::vector<double> x_diff;
  x_diff.resize(num_point_ - 1);
  for (int i = 0; i < num_point_ - 1; ++i) {
    x_diff[i] = x_[i + 1] - x_[i];
  }

  // prepare linear equations, "A"
  Eigen::MatrixXd A = Eigen::MatrixXd::Zero(num_point_, num_point_);
  for (int i = 0; i < num_point_ - 1; ++i) {
    if (i != num_point_ - 2) {
      A(i + 1, i + 1) = 2. * (x_diff[i] + x_diff[i + 1]);
    }
    A(i + 1, i) = x_diff[i];
    A(i, i + 1) = x_diff[i];
  }
  A(0, 0) = 1.;
  A(0, 1) = 0.;
  A(num_point_ - 1, num_point_ - 2) = 0.;
  A(num_point_ - 1, num_point_ - 1) = 1.;

  // prepare linear equations, "b"
  Eigen::VectorXd b(num_point_);
  b(0) = 0.;
  b(num_point_ - 1) = 0.;
  for (int i = 0; i < num_point_ - 2; ++i) {
    b(i + 1) = 6. * (y_[i + 2] - y_[i + 1]) / x_diff[i + 1] -
               6. * (y_[i + 1] - y_[i]) / x_diff[i];
  }

  // solve linear equations, "A\b"
  Eigen::VectorXd m = A.householderQr().solve(b);
  // Eigen::VectorXd m = A.colPivHouseholderQr().solve(b);
  // Eigen::VectorXd m = A.ldlt().solve(b);

  // calculate cubic spline coefficients
  coefficient0_ = y_;
  coefficient1_.resize(num_point_ - 1);
  coefficient2_.resize(num_point_);
  coefficient3_.resize(num_point_ - 1);
  for (int i = 0; i < num_point_; ++i) {
    coefficient2_[i] = m(i) / 2.0;
  }
  for (int i = 0; i < num_point_ - 1; ++i) {
    coefficient1_[i] = (y_[i + 1] - y_[i]) / x_diff[i] - x_diff[i] / 2. * m(i) -
                       x_diff[i] / 6. * (m(i + 1) - m(i));
    coefficient3_[i] = (m(i + 1) - m(i)) / (6 * x_diff[i]);
  }
}

double CubicSpline::EvaluateCubicSpline(const double& x) {
  auto lower = std::lower_bound(x_.begin(), x_.end(), x);
  if (lower != x_.begin()) {
    lower--;
  }
  double dx = x - *lower;
  auto i = std::min(num_point_ - 2,
                    static_cast<int>(std::distance(x_.begin(), lower)));
  return coefficient0_[i] + coefficient1_[i] * dx +
         coefficient2_[i] * std::pow(dx, 2) +
         coefficient3_[i] * std::pow(dx, 3);
}

double CubicSpline::EvaluateFirstDerivative(const double& x) {
  auto lower = std::lower_bound(x_.begin(), x_.end(), x);
  if (lower != x_.begin()) {
    lower--;
  }
  double dx = x - *lower;
  auto i = std::min(num_point_ - 2,
                    static_cast<int>(std::distance(x_.begin(), lower)));
  return coefficient1_[i] + 2. * coefficient2_[i] * dx +
         3. * coefficient3_[i] * std::pow(dx, 2);
}

double CubicSpline::EvaluateSecondDerivative(const double& x) {
  auto lower = std::lower_bound(x_.begin(), x_.end(), x);
  if (lower != x_.begin()) {
    lower--;
  }
  double dx = x - *lower;
  auto i = std::min(num_point_ - 2,
                    static_cast<int>(std::distance(x_.begin(), lower)));
  return 2. * coefficient2_[i] + 6. * coefficient3_[i] * dx;
}





double CubicSpline2D::init(std::vector<double> x, std::vector<double> y)
{
    s.clear();
    calc_s(x,y,s);
    sx.Build(s,x);
    sy.Build(s,y);

    double x_l,y_l,heading_l,curvature_l;
//    printf("======================111111 %f %d %d\r\n",s.back(),x.size(),y.size());
//    for(int i = 0;i<x.size();i++)
//    {
//        printf("%f %f\r\n",x.at(i),y.at(i));
//    }
    for(double i=0;i<s.back();)
    {
        i = i+0.5;
        calc_position(i,x_l,y_l);
        calc_curvature(i,curvature_l);
        calc_yaw(i,heading_l);
        x_.push_back(x_l);
        y_.push_back(y_l);
        heading_.push_back(heading_l);
        curvature_.push_back(curvature_l);
//        printf("%f %f\r\n",x_l,y_l);
    }
//    printf("======================2222222\r\n");

    return s.back();
}

 std::vector<GPSPoint> CubicSpline2D::GetGpsPoints()
{
    std::vector<GPSPoint> points;
    GPSPoint point;
    points.clear();
//    printf("x size %d\r\n",x_.size());
    for(int i=0;i<x_.size();i++)
    {
        point.x_ = x_.at(i);
        point.y_ = y_.at(i);
        point.k_ = curvature_.at(i);
        point.theta_ = heading_.at(i);
        point.s_ = s.at(i);
        points.push_back(point);
//        printf("%f %f\r\n",point.x_,point.y_);
    }

    return points;
}

void CubicSpline2D::calc_s(std::vector<double> &x, std::vector<double> &y, std::vector<double> &s)
{

    if((x.size() != y.size()) || x.empty())
        return;

    s.emplace_back(0.);
    double dx = x[1]-x[0];
    double dy = y[1]-y[0];

    for(int kk =1;kk<x.size();kk++)
    {
        double dx = x[kk] - x[kk-1];
        double dy = y[kk] - y[kk-1];
        s.emplace_back(s.back()+std::hypot(dy, dx));
    }

}

void CubicSpline2D::calc_position(double &s, double &x, double &y)
{
    x = sx.EvaluateCubicSpline(s);
    y = sy.EvaluateCubicSpline(s);
}

void CubicSpline2D::calc_curvature(double &s,double &k)
{
    double dx = sx.EvaluateFirstDerivative(s);
    double ddx = sx.EvaluateSecondDerivative(s);
    double dy = sy.EvaluateFirstDerivative(s);
    double ddy = sy.EvaluateSecondDerivative(s);
    k = (ddy * dx - ddx * dy) / (std::pow(dx, 2) + std::pow(dy, 2));
}

void CubicSpline2D::calc_yaw(double &s,double &heading)
{
    double dx = sx.EvaluateFirstDerivative(s);
    double dy = sy.EvaluateFirstDerivative(s);
    heading = std::atan2(dy,dx);
}


double CubicSpline2D::get_s(float x,float y, float heading)
{
    double ss = 0;
    if (x_.empty() || y_.empty()) {
      return 0;
    }
      auto min_distance = std::numeric_limits<double>::infinity();
      int min_index = 0;
      for (size_t j = 0; j < x_.size(); ++j) {
        double heading_error = heading_[j] - heading;
        if (fabs(heading_error) > M_PI) {
          heading_error =
              std::atan2(std::sin(heading_error), std::cos(heading_error));
        }
        if (std::fabs(heading_error) > M_PI / 2) {
          continue;
        }
        auto distance = hypot(x_[j] - x, y_[j] - y);
        if (distance < min_distance) {
          min_distance = distance;
          min_index = j;   //找到提取路段的最近的点
        }

    }
      for(int kk =1;kk<min_index+1;kk++)
        {
            double dx = x_[kk] - x_[kk-1];
            double dy = y_[kk] - y_[kk-1];
            ss = ss+std::hypot(dy, dx);
        }

    return ss;

}
}
}
}
